#!/usr/bin/env python
import Jetson.GPIO as GPIO
import time
import rospy
from sensor_msgs.msg import Joy

GPIO.setmode(GPIO.BCM)
PIN1=[21,20,26]
GPIO.setup(PIN1, GPIO.OUT,initial=GPIO.LOW)

motor_msg=0

def motor_forward():
    motor_stop()
    GPIO.output(20, GPIO.HIGH)

def motor_stop():
    GPIO.output([21,20],GPIO.LOW)

def motor_backward():
    motor_stop()
    GPIO.output(21,GPIO.HIGH)

def motor_start():
    GPIO.output(26,GPIO.HIGH)

def motor_stopB():
    GPIO.output(26,GPIO.LOW)


def call_back_test(msg):
    global motor_msg
    # if msg.button[0] :
    #     motor_msg=0
    # if msg.button[1] :
    #     motor_msg=1
    if msg.button[2] :
        motor_msg=2
    if msg.button[3] :
        motor_msg=3


def main():
    rospy.init_node('screw_motor_controller')
    rospy.Subscriber('/iiwa/state/CartesianPose_end',Joy, callback=call_back_test, queue_size=1)
    rate = rospy.Rate(300)
    while not rospy.is_shutdown():
        try:
            if(motor_msg==1) :
                motor_forward()
            if(motor_msg==0):
                motor_stop()
            if(motor_msg==-1):
                motor_backward()
            if(motor_msg==2):
                motor_start()
            if(motor_msg==3):
                motor_stopB()
        except:
            motor_stop()
            motor_stopB()
            GPIO.cleanup()
        rate.sleep()
    return

if __name__ == '__main__':
    main()